﻿#pragma once

#include <learn/geom/vec.h>
#include <learn/math/math.h>
#include <learn/math/matrix.h>

namespace xi
{

namespace alg
{

using namespace xi::math;
using namespace xi::geom;

template <std::size_t N> class Alg_DynamicCurve
{
    static_assert(N == 2 || N == 3, "N must be 2 or 3");
    using Vec = VecT<N, double>;

  public:
    /**
     * @brief 动力学求值
     *
     * @param[in] X 控制点
     * @param[in] h 时间步长
     */
    Alg_DynamicCurve(const std::vector<Vec> &X, double h) : h(h), X0(X.size(), 1), MB(X.size(), X.size())
    {
    }

    void prepare(const std::vector<Vec> &X, const MatrixXd &Mh)
    {
        std::size_t n = X.size() - 1;

        MatrixXd MX(n + 1, n + 1);
        MX.set_zero();

        // MX = [x0 x1 ... xn-3 xn-2 xn-1 xn]
        //      [y0 y1 ... yn-3 yn-2 yn-1 yn]
        //      [z0 z1 ... zn-3 zn-2 zn-1 zn]
        //      [1  0  ... 0    0    0    0 ]
        //      [0  1  ... 0    0    0    0 ]
        //      [      ... 0    0    0    0 ]
        //      [0  0  ... 1    0    0    0 ]
        for (int i = 0; i < n + 1; i++)
        {
            for (int j = 0; j < N; j++)
                MX(j, i) = X[i][j];
        }
        for (int i = N; i < n + 1; i++)
            MX(i, i - N) = 1;

        // 记录迭代矩阵
        MB = MX * Mh * MX.invert();

        // MX = (X0, X1, ..., Xn)
        for (int i = 0; i < N; i++)
            X0(i, 0) = X[0][i];
        X0(N, 0) = 1;
    }

    /**
     * @brief 迭代计算
     *
     * @param[in] t
     * @return std::vector<Vec>
     */
    std::vector<Vec> evaluate(double t) const
    {
        // 计算迭代次数
        std::size_t n = t / h;
        std::vector<Vec> result(n);

        // X1 = MB * X0, Phi(ti) = MB * Phi(ti-1)
        MatrixXd X1 = X0;
        for (int i = 0; i < n; i++)
        {
            X1 = MB * X1;
            for (int j = 0; j < N; j++)
                result[i][j] = X1(j, 0);
        }
        return result;
    }

  public:
    double h;
    MatrixXd X0; // X0 = Phi(0)
    MatrixXd MB; // Phi(ti) = MB * Phi(ti-1)
};

template <std::size_t N> class Alg_DynamicPowerCurve : public Alg_DynamicCurve<N>
{
    using Vec = VecT<N, double>;

  public:
    /**
     * @brief 幂基函数动力学求值
     *
     * @param[in] X 控制点
     * @param[in] h 时间步长
     */
    Alg_DynamicPowerCurve(const std::vector<Vec> &X, double h) : Alg_DynamicCurve<N>(X, h)
    {
        std::size_t n = X.size() - 1;
        if (n < N)
            return;

        MatrixXd Mh(n + 1, n + 1);
        Mh.set_zero();

        // Un(t + h) = Mh * Un(t)
        for (int k = 0; k < n + 1; k++)
        {
            for (int l = 0; l < n + 1 - k; l++)
                Mh(k + l, l) = std::pow(h, k) * binomial(k + l, k);
        }

        // 装载矩阵
        this->prepare(X, Mh);
    }
};

template <std::size_t N> class Alg_DynamicBezierCurve : public Alg_DynamicCurve<N>
{
    using Vec = VecT<N, double>;

  public:
    /**
     * @brief Bezier 动力学求值
     *
     * @param[in] X 控制点
     * @param[in] h 时间步长
     */
    Alg_DynamicBezierCurve(const std::vector<Vec> &X, double h) : Alg_DynamicCurve<N>(X, h)
    {
        std::size_t n = X.size() - 1;
        if (n < N)
            return;

        auto bernstein = [](int i, int n, double t) -> double {
            return binomial(n, i) * std::pow(t, i) * std::pow(1 - t, n - i);
        };

        MatrixXd CB(n + 1, n + 1);
        CB.set_zero();

        // Phi((1 - t) * a + t * b) = CB * Phi(t)
        double a = h, b = 1 + h; // 这里 a, b 可以取其它值，只有当 b - a = 1 时，迭代步长才是恒定的
        for (int k = 0; k < n + 1; k++)
        {
            for (int l = 0; l < n + 1; l++)
            {
                double c = 0;
                for (int i = 0; i <= l; i++)
                {
                    for (int j = 0; j <= n - l; j++)
                    {
                        if (i + j == k)
                            c += bernstein(i, l, b) * bernstein(j, n - l, a);
                    }
                }
                CB(k, l) = c;
            }
        }

        // 装载矩阵
        this->prepare(X, CB);
    }
};

} // namespace alg

} // namespace xi